import serial
import sys
from struct import unpack

def ThorSer():

    # check for serial port argument
    if len(sys.argv) < 2:
        print "Specify serial port to use"
        exit()

    # open serial port and print info
    ser = serial.Serial((int(sys.argv[1])-1), 115200, timeout=1)

    idx = 0
    error = 0
    count = 5
    MSGSIZE = 29
 
    # drive, steer
    #mtrs = "+064" + "\0" + "9" + "\0"
    mtrs = "+000" + "\0" + "9" + "\0"

    # grip sensor, grip position
    if sys.argv[2] == "open":
        grip = "000" + "\0" + "014" + "\0"
    if sys.argv[2] == "close":
        grip = "000" + "\0" + "007" + "\0"
        
    # front, rear
    bump = "0" + "\0" + "0" + "\0"

    # cam tilt
    cam = "000" + "\0"

    # red, yellow, green
    lights = "1" + "\0" + "0" + "\0" + "1" + "\0"
    
    data = mtrs + grip + bump + cam + lights
 
    ser.write('W')
    ser.write(data)
 
    # write and read messages for a while
    while idx < count:

        print "index ",idx	
            
        # read and print incoming serial message
        ser.write('R')
        
        # discard serial bytes until framing byte is detected
        while ser.read(1) != 'F':
            pass
            
        inMsg = ser.read(MSGSIZE)
        print len(inMsg)
        print inMsg
        
        idx += 1

    null = "+000" + "\0" + "0" + "\0" + "000" + "\0" + "000" + "\0" + "0" + "\0" + "0" + "\0" + "000" + "\0" + "0" + "\0" + "0" + "\0" + "0" + "\0"
        
    #ser.write('W')
    #ser.write(null)
    
if __name__ == "__main__":
    ThorSer()